#include "pid_controller.h"

void PID_Init(pid_t *pid, float Kp, float Ki, float Kd) 
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->prevError = 0.0;
    pid->integral = 0.0;
}

float PID_Calculate(pid_t *pid, float setpoint, float measuredValue, float dt) 
{
    float error = setpoint - measuredValue;

    pid->integral += error * dt;

    float derivative = (error - pid->prevError) / dt;

    float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;

    pid->prevError = error;
    
    return output;
}
